﻿//
// RobotController.cs
//
// Authors:
//    Claus Jørgensen <10229@iha.dk>
//    Jens Bloch Christensen <09584@iha.dk>
//    Jonas Daugaard Møller <08814@iha.dk>
//
using System;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Threading.Tasks;
using System.Threading;
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.Diagnostics.Contracts;

namespace RSD.Robot
{
    /// <summary>
    /// Implementation of the methods required for using the Intelitek USBC-Controller.
    /// </summary>
    public class RobotController : USBC, IRobotController
    {
        #region Callbacks

        /// <summary>
        /// Initialization Complete callback.
        /// </summary>
        private InitializationCompleteCallback initializationComplete;

        /// <summary>
        /// Error callback.
        /// </summary>
        private ErrorCallback error;

        /// <summary>
        /// Motion Started callback.
        /// </summary>
        private MovementCallback motionStarted;

        /// <summary>
        /// Motion Ended callback.
        /// </summary>
        private MovementCallback motionEnded;

        /// <summary>
        /// Control Status Changed callback.
        /// </summary>
        private WatchControlCallback controlStatusChanged;

        /// <summary>
        /// Homing Status Changed callback.
        /// </summary>
        private HomingNotifierCallback homingStatusChanged;

        /// <summary>
        /// Join Status Changed callback.
        /// </summary>
        private WatchJointCallback jointStatusChanged;

        /// <summary>
        /// Digital Input Recieved callback.
        /// </summary>
        private IOMonitorCallback digitalInputReceived;

        /// <summary>
        /// Digital Output Recieved callback.
        /// </summary>
        private IOMonitorCallback digitalOutputReceived;

        #endregion

        #region Queue Members

        /// <summary>
        /// Internal queue of robot actions.
        /// </summary>
        private Queue<Action> actionQueue = new Queue<Action>();

        /// <summary>
        /// Indicates whether the robot is moving.
        /// </summary>
        private bool isMoving = false;

        #endregion

        #region Constructor and Destructor

        /// <summary>
        /// Initializes a new instance of the <see cref="T:RobotController"/> class.
        /// </summary>
        public RobotController()
        {
            this.initializationComplete = new InitializationCompleteCallback(OnInitializationCompleted);
            this.error = new ErrorCallback(OnError);
            this.motionStarted = new MovementCallback(OnMotionStarted);
            this.motionEnded = new MovementCallback(OnMotionEnded);
            this.controlStatusChanged = new WatchControlCallback(OnControlStatusChanged);
            this.homingStatusChanged = new HomingNotifierCallback(OnHomingStatusChanged);
            this.jointStatusChanged = new WatchJointCallback(OnJointStatusChanged);
            this.digitalInputReceived = new IOMonitorCallback(OnDigitalInputReceived);
            this.digitalOutputReceived = new IOMonitorCallback(OnDigitalOutputReceived);
        }

        /// <summary>
        /// Finalizes an instance of the <see cref="T:RobotController"/> class.
        /// </summary>
        ~RobotController()
        {
            Dispose(false);
        }

        #endregion

        #region IDisposable Members

        /// <summary>
        /// Releases all resources used by the <see cref="T:USBC"/> object.
        /// </summary>
        public void Dispose()
        {
            Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Releases all resources used by the <see cref="T:USBC"/> object.
        /// </summary>
        /// <param name="disposing">True if the call was from the user's code, or false if called from a finalizer.</param>
        private void Dispose(bool disposing)
        {
            if (disposing)
            {
                try
                {
                    Close();
                }
                catch (DllNotFoundException e)
                {
                    Trace.TraceError(e.Message);
                }
            }
        }

        #endregion

        #region IRobotController Members

        /// <summary>
        /// Gets or sets the folder which contains the robot configuration files.
        /// </summary>
        public string ParameterFolder
        {
            get
            {
                string folderName = string.Empty;
                NativeMethods.GetParameterFolder(folderName);

                return folderName;
            }
            set
            {
                NativeMethods.SetParameterFolder(value);
            }
        }

        /// <summary>
        /// Gets the robot version number.
        /// </summary>
        public uint Version
        {
            get
            {
                uint version = default(uint);
                NativeMethods.GetVersion(version);

                return version;
            }
        }

        /// <summary>
        /// Gets a value indicating whether the emergency button has activated or not.
        /// </summary>
        public bool IsEmergency
        {
            get
            {
                return NativeMethods.IsEmergency();
            }
        }

        /// <summary>
        /// Gets a value indicating whether the robot is in teaching mode or not.
        /// </summary>
        public bool IsInTeachingMode
        {
            get
            {
                return NativeMethods.IsInTeachingMode();
            }
        }

        /// <summary>
        /// Gets a value indicating whether the robot is online.
        /// </summary>
        public bool IsOnline
        {
            get
            {
                return NativeMethods.IsOnline();
            }
        }

        /// <summary>
        /// Gets the width between the grippers fingers.
        /// </summary>
        public Tuple<short, short> Jaw
        {
            get
            {
                short perc = default(short);
                short metricValue = default(short);

                OnActionStarted();

                NativeMethods.GetJaw(out perc, out metricValue);

                Thread.Sleep(1000);
                OnActionCompleted();

                return Tuple.Create(perc, metricValue);
            }
        }

        /// <summary>
        /// Gets the configuration for the connected robot.
        /// </summary>
        public USBCConfiguration Configuration
        {
            get
            {
                var config = new USBCConfiguration();

                NativeMethods.GetConfiguation(config);

                return config;
            }
        }

        /// <summary>
        /// Finds the USB device connected to the robot and initializes the connection
        /// between the robot and the computer, using the default <see cref="SystemType"/>.
        /// </summary>
        /// <param name="mode">Initialization mode.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool Initialization(InitializationMode mode)
        {
            return Initialization(mode, SystemType.Default);
        }

        /// <summary>
        /// Finds the USB device connected to the robot and initializes the connection
        /// between the robot and the computer.
        /// </summary>
        /// <param name="mode">Initialization mode.</param>
        /// <param name="systemType">System type.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool Initialization(InitializationMode mode, SystemType systemType)
        {
            if (initializationComplete == null || error == null)
            {
                throw new InvalidOperationException();
            }

            var initialized = NativeMethods.Initialization((short)mode,
                (short)systemType, initializationComplete, error);

            if (initialized)
            {
                MonitorDigitalInput();
                MonitorDigitalOutput();
                MonitorControlStatus();
                MonitorHomingStatus();
                MonitorJointStatus();
                MonitorMotion();
            }

            return initialized;
        }

        /// <summary>
        /// Closes the connection to the USBC Controller.
        /// </summary>
        public void Close()
        {
            NativeMethods.CloseUSBC();
        }

        /// <summary>
        /// Monitors the digital input.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorDigitalInput()
        {
            if (digitalInputReceived == null)
            {
                throw new InvalidOperationException();
            }

            return NativeMethods.BeginMonitorDigitalInput(digitalInputReceived);
        }

        /// <summary>
        /// Monitors the digital output.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorDigitalOutput()
        {
            if (digitalOutputReceived == null)
            {
                throw new InvalidOperationException();
            }

            return NativeMethods.BeginMonitorDigitalOutput(digitalOutputReceived);
        }

        /// <summary>
        /// Gets the digital input.
        /// </summary>
        /// <returns>the digital input.</returns>
        public uint GetDigitalInput()
        {
            uint statusBitmap = default(uint);
            NativeMethods.GetDigitalInput(statusBitmap);

            return statusBitmap;
        }

        /// <summary>
        /// Gets the digital output.
        /// </summary>
        /// <returns>the digital output.</returns>
        public uint GetDigitalOutput()
        {
            uint statusBitmap = default(uint);
            NativeMethods.GetDigitalOutput(statusBitmap);

            return statusBitmap;
        }

        /// <summary>
        /// Monitors the robots movement.
        /// </summary>
        public void MonitorMotion()
        {
            if (motionStarted == null || motionEnded == null)
            {
                throw new InvalidOperationException();
            }

            NativeMethods.WatchMotion(motionEnded, motionStarted);
        }

        /// <summary>
        /// Sets the time future movement should take, for the given axis.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="time">Movement time.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SetTime(Axis axis, TimeSpan time)
        {
            return NativeMethods.Time((byte)axis, (int)time.TotalMilliseconds);
        }

        /// <summary>
        /// Sets the speed future movement should take, for the given axis.
        /// </summary>
        /// <remarks>
        /// A <paramref name="speed"/> of 0 will cause the movement to stop.
        /// </remarks>
        /// <param name="axis">Axis to move.</param>
        /// <param name="speed">Movement speed (0-10).</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SetSpeed(Axis axis, int speed)
        {
            if (speed < 0 || speed > 10)
            {
                throw new ArgumentException("Speed must be between 0 and 10");
            }

            return NativeMethods.Speed((byte)axis, speed);
        }

        /// <summary>
        /// Moves the robot by joints.
        /// </summary>
        /// <param name="vector">Vector to move to.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveJoint(Vector vector)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            return NativeMethods.MoveJoint(vector.Name, vector.Dimension, null, 0);
        }

        /// <summary>
        /// Moves the robot by joints.
        /// </summary>
        /// <param name="vectorA">First vector to move to.</param>
        /// <param name="vectorB">Second vector to move to.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveJoint(Vector vectorA, Vector vectorB)
        {
            if (vectorB == null)
            {
                throw new ArgumentNullException("vectorA");
            }

            if (vectorA == null)
            {
                throw new ArgumentNullException("vectorB");
            }

            return NativeMethods.MoveJoint(vectorA.Name, vectorA.Dimension, vectorB.Name, vectorB.Dimension);
        }

        /// <summary>
        /// Moves the robot in a linear path.
        /// </summary>
        /// <param name="vector">Vector to move to.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveLinear(Vector vector)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            return NativeMethods.MoveLinear(vector.Name, vector.Dimension, null, 0);
        }

        /// <summary>
        /// Moves the robot in a linear path.
        /// </summary>
        /// <param name="vectorA">First vector to move to.</param>
        /// <param name="vectorB">Second vector to move to.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveLinear(Vector vectorA, Vector vectorB)
        {
            if (vectorA == null)
            {
                throw new ArgumentNullException("vectorA");
            }

            if (vectorB == null)
            {
                throw new ArgumentNullException("vectorB");
            }

            return NativeMethods.MoveLinear(vectorA.Name, vectorA.Dimension,
                vectorB.Name, vectorB.Dimension);
        }

        /// <summary>
        /// Moves the conveyor belt.
        /// </summary>
        /// <param name="velocity">Velocity of the conveyor belt in percent.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MoveConveyorBelt(int velocity)
        {
            if (velocity <= -100 || velocity >= 100)
            {
                throw new ArgumentException("velocity must be between 0 and 100");
            }

            NativeMethods.EnterManual(0);
            return NativeMethods.MoveManual((byte)Axis.ConveyorBelt, velocity);
        }

        /// <summary>
        /// Waits for conveyor belt to send input.
        /// </summary>
        public void WaitForConveyorBelt()
        {
            DigitalInputReceived += RobotControllerQueue_DigitalInputReceived;
        }

        /// <summary>
        /// Moves to a position.
        /// </summary>
        /// <param name="position">Position.</param>
        /// <param name="pointType">Point type.</param>
        public void MoveToPosition(Position position, PointType pointType)
        {
            if (position == null)
            {
                throw new ArgumentNullException("position");
            }

            string vectorName = Guid.NewGuid().ToString();

            if (string.IsNullOrEmpty(vectorName))
            {
                throw new InvalidOperationException("Error when creating a GUID for the vectorName.");
            }

            var vector = DefineVector(Axis.Robot, vectorName);
            vector.Position = position;

            switch (pointType)
            {
                case PointType.RelativeJointCoordinate:
                case PointType.AbsoluteJointCoordinate:
                    SetJoints(vector, vector.Position, pointType);
                    break;
                case PointType.AbsoluteXYZCoordinateA:
                case PointType.AbsoluteXYZCoordinateB:
                case PointType.RelativeXYZCoordinate:
                    Teach(vector, vector.Position, pointType);
                    break;
            }

            MoveJoint(vector);
        }

        /// <summary>
        /// Opens the gripper.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool OpenGripper()
        {
            OnActionStarted();
            
            bool result = NativeMethods.OpenGripper();
            
            Thread.Sleep(3000);
            OnActionCompleted();

            return result;
        }

        /// <summary>
        /// Closes the gripper.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool CloseGripper()
        {
            OnActionStarted();

            bool result = NativeMethods.CloseGripper();

            Thread.Sleep(3000); 
            OnActionCompleted();

            return result;
        }

        /// <summary>
        /// Stops all movement on the given axis.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool StopMovement(Axis axis)
        {
            return NativeMethods.Stop((byte)axis);
        }

        /// <summary>
        /// Sets the default velocity of the given axis.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="velocity">Velocity in percent.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SetVelocity(Axis axis, short velocity)
        {
            if (velocity < 0 || velocity > 100)
            {
                throw new ArgumentException("velocity must be between 0 and 100");
            }

            return NativeMethods.Velocity((byte)axis, velocity);
        }

        /// <summary>
        /// Turns control on or off for the given axis.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="isOn">true to turn on, false to turn off.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SetControl(Axis axis, bool isOn)
        {
            return NativeMethods.Control((byte)axis, isOn);
        }

        /// <summary>
        /// Monitors the control status.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorControlStatus()
        {
            if (controlStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            return NativeMethods.WatchControl(controlStatusChanged);
        }

        /// <summary>
        /// Monitors the homing process.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorHomingStatus()
        {
            if (homingStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            return NativeMethods.WatchHoming(homingStatusChanged);
        }

        /// <summary>
        /// Homes the robot.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool Home(Axis axis)
        {
            if (homingStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            return NativeMethods.Home((byte)axis, homingStatusChanged);
        }

        /// <summary>
        /// Sets the home position to the current position.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SetHome(Axis axis)
        {
            return NativeMethods.SetHome((byte)axis);
        }

        /// <summary>
        /// Monitors the values for each joint on the robot.
        /// </summary>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool MonitorJointStatus()
        {
            if (jointStatusChanged == null)
            {
                throw new InvalidOperationException();
            }

            return NativeMethods.WatchJoint(jointStatusChanged);
        }

        /// <summary>
        /// Loads points into the robots memory from a file.
        /// </summary>
        /// <param name="fileName">The file to open.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool LoadPointsFromFile(string fileName)
        {
            if (string.IsNullOrEmpty(fileName))
            {
                throw new ArgumentException("fileName cannot be null or empty");
            }

            if (File.Exists(fileName))
            {
                return NativeMethods.LoadPointsFromFile(fileName);
            }
            else
            {
                throw new FileNotFoundException("File Not found", fileName);
            }
        }

        /// <summary>
        /// Defines a vector in the robots memory.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="vectorName">Name of the vector.</param>
        /// <returns>
        /// a vector with the given vectorname; otherwise, null.
        /// </returns>
        public Vector DefineVector(Axis axis, string vectorName)
        {
            if (string.IsNullOrEmpty(vectorName))
            {
                throw new ArgumentException("vectorName cannot be null or empty.");
            }

            short dimension = 1;

            if (NativeMethods.DefineVector((byte)axis, vectorName, dimension))
            {
                return new Vector(axis, vectorName, dimension);
            }
            else
            {
                return null;
            }
        }

        /// <summary>
        /// Renames a vector in the robots memory.
        /// </summary>
        /// <param name="oldName">Old vector name.</param>
        /// <param name="newName">New vector name.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool RenameVector(string oldName, string newName)
        {
            if (string.IsNullOrEmpty(oldName))
            {
                throw new ArgumentException("oldName cannot be null or empty");
            }

            if (string.IsNullOrEmpty(newName))
            {
                throw new ArgumentException("newName cannot be null or empty");
            }

            return NativeMethods.RenameVector(oldName, newName);
        }

        /// <summary>
        /// Deletes all points from a vector in the robots memory.
        /// </summary>
        /// <param name="vector">Vector to delete.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool DeleteVector(Vector vector)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            return NativeMethods.DeletePoint(vector.Name, -1);
        }

        /// <summary>
        /// Deletes a vector point in the robots memory.
        /// </summary>
        /// <param name="vector">Vector to delete a point from.</param>
        /// <param name="point">Point to remove from the given vector.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool DeleteVector(Vector vector, short point)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            return NativeMethods.DeletePoint(vector.Name, point);
        }

        /// <summary>
        /// Saves the current position as a point in memory.
        /// </summary>
        /// <param name="vector">Vector to save the position to.</param>
        /// <param name="pointType">Location point type.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool SaveCurrentLocation(Vector vector, PointType pointType)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            return NativeMethods.Here(vector.Name, vector.Dimension, (int)pointType);
        }

        /// <summary>
        /// Sets the joints.
        /// </summary>
        /// <param name="vector">The vector.</param>
        /// <param name="position">The position.</param>
        /// <param name="pointType">Type of the point.</param>
        /// <returns></returns>
        public bool SetJoints(Vector vector, Position position, PointType pointType)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            if (position == null)
            {
                throw new ArgumentNullException("position");
            }

            vector.Position = position;

            return NativeMethods.SetJoints(vector.Name, 1,
                position.Joint, (short)position.Joint.Length, (int)pointType);
        }

        /// <summary>
        /// Teaches a robot the location of a named vector.
        /// </summary>
        /// <param name="vector">Vector to teach.</param>
        /// <param name="position">XYZ position to associate the vector with.</param>
        /// <param name="pointType">Location point type.</param>
        /// <returns>
        /// <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </returns>
        public bool Teach(Vector vector, Position position, PointType pointType)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            if (position == null)
            {
                throw new ArgumentNullException("position");
            }

            vector.Position = position;

            return NativeMethods.Teach(vector.Name, 1,
                position.XYZ, (short)position.XYZ.Length, (int)pointType);
        }

        /// <summary>
        /// Gets the point information for a given vector and point-number.
        /// </summary>
        /// <param name="vector">Vector to get point information for.</param>
        /// <param name="pointType">Location point type.</param>
        /// <returns>point information for the given location.</returns>
        public Position GetPointInfo(Vector vector, PointType pointType)
        {
            if (vector == null)
            {
                throw new ArgumentNullException("vector");
            }

            int[] enc = new int[TotalAxes];
            int[] xyz = new int[TotalAxes];

            NativeMethods.GetPointInfo(vector.Name, vector.Dimension, enc, xyz, (int)pointType);

            return new Position()
            {
                Enc = enc,
                XYZ = xyz
            };
        }

        /// <summary>
        /// Gets the current position of the robot.
        /// </summary>
        /// <returns>the robots current position.</returns>
        public Position GetCurrentPosition()
        {
            int[] enc = new int[TotalAxes];
            int[] joint = new int[TotalAxes];
            int[] xyz = new int[TotalAxes];

            NativeMethods.GetCurrentPosition(enc, joint, xyz);

            return new Position()
            {
                Enc = enc,
                Joint = joint,
                XYZ = xyz
            };
        }

        #endregion

        #region IRobotController Action Queue

        /// <summary>
        /// Enqueues any RobotController action.
        /// </summary>
        /// <param name="item">Action delegate.</param>
        public void EnqueueAction(Action<IRobotController> item)
        {
            actionQueue.Enqueue(() => item(this));
        }

        /// <summary>
        /// Executes the movement queue.
        /// </summary>
        public void RunActionQueue()
        {
            MotionStarted += RobotControllerQueue_MotionStarted;
            MotionEnded += RobotControllerQueue_MotionEnded;
            InitializationCompleted += RobotControllerQueue_InitializationCompleted;
            HomingStatusChanged += RobotControllerQueue_HomingStatusChanged;
            ActionStarted += RobotControllerQueue_ActionStarted;
            ActionCompleted += RobotControllerQueue_ActionCompleted;

            while (actionQueue.Count > 0)
            {
                if (!isMoving)
                {
                    isMoving = true;

                    Thread.Sleep(50); // Safety pause.

                    var action = actionQueue.Dequeue();
                    action();
                }
            }

            ActionStarted -= RobotControllerQueue_ActionStarted;
            ActionCompleted -= RobotControllerQueue_ActionCompleted;
            InitializationCompleted -= RobotControllerQueue_InitializationCompleted;
            HomingStatusChanged -= RobotControllerQueue_HomingStatusChanged;
            MotionStarted -= RobotControllerQueue_MotionStarted;
            MotionEnded -= RobotControllerQueue_MotionEnded;
        }

        /// <summary>
        /// Triggers the ActionCompleted event.
        /// </summary>
        public void TriggerActionCompleted()
        {
            OnActionCompleted();
        }

        private void RobotControllerQueue_ActionStarted(object sender, EventArgs e)
        {
            isMoving = true;
        }

        private void RobotControllerQueue_ActionCompleted(object sender, EventArgs e)
        {
            isMoving = false;
        }

        private void RobotControllerQueue_HomingStatusChanged(object sender, HomingStatusEventArgs e)
        {
            isMoving = false;
        }

        private void RobotControllerQueue_InitializationCompleted(object sender, InitializationCompletedEventArgs e)
        {
            isMoving = false;
        }

        /// <summary>
        /// Handles the MotionStarted event of the RobotController control.
        /// </summary>
        /// <param name="sender">The source of the event.</param>
        /// <param name="e">The <see cref="RSD.Robot.MotionEventArgs"/> instance containing the event data.</param>
        private void RobotControllerQueue_MotionStarted(object sender, MotionEventArgs e)
        {
            isMoving = true;
        }

        /// <summary>
        /// Handles the MotionEnded event of the RobotController control.
        /// </summary>
        /// <param name="sender">The source of the event.</param>
        /// <param name="e">The <see cref="RSD.Robot.MotionEventArgs"/> instance containing the event data.</param>
        private void RobotControllerQueue_MotionEnded(object sender, MotionEventArgs e)
        {
            isMoving = false;
        }

        /// <summary>
        /// Handles the DigitalInputReceived event of the RobotController control.
        /// </summary>
        /// <param name="sender">The source of the event.</param>
        /// <param name="e">The <see cref="RSD.Robot.IOEventArgs"/> instance containing the event data.</param>
        private void RobotControllerQueue_DigitalInputReceived(object sender, IOEventArgs e)
        {
            StopMovement(Axis.Pheripals);
            StopMovement(Axis.ConveyorBelt);

            isMoving = false;

            DigitalInputReceived -= RobotControllerQueue_DigitalInputReceived;
        }

        #endregion

        #region IRobotControllerAsync Members

        /// <summary>
        /// Finds the USB device connected to the robot and initializes the connection
        /// between the robot and the computer, using the default SystemType.
        /// </summary>
        /// <param name="mode">Initialization mode.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void InitializationAsync(InitializationMode mode, Action<bool> callback = null)
        {
            InitializationAsync(mode, SystemType.Default, callback);
        }

        /// <summary>
        /// Finds the USB device connected to the robot and initializes the connection
        /// between the robot and the computer.
        /// </summary>
        /// <param name="mode">Initialization mode.</param>
        /// <param name="systemType">System type.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void InitializationAsync(InitializationMode mode, SystemType systemType, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => Initialization(mode, systemType));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Homes the robot.
        /// </summary>
        /// <param name="axis">Axis to move.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void HomeAsync(Axis axis, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => Home(axis));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Moves the robot by joints.
        /// </summary>
        /// <param name="vector">Vector to move to.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void MoveJointAsync(Vector vector, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => MoveJoint(vector));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Moves the robot by joints.
        /// </summary>
        /// <param name="vectorA">First vector to move to.</param>
        /// <param name="vectorB">Second vector to move to.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void MoveJointAsync(Vector vectorA, Vector vectorB, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => MoveJoint(vectorA, vectorB));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Moves the robot in a linear path.
        /// </summary>
        /// <param name="vector">Vector to move to.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void MoveLinearAsync(Vector vector, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => MoveLinear(vector));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Moves the robot in a linear path.
        /// </summary>
        /// <param name="vectorA">First vector to move to.</param>
        /// <param name="vectorB">Second vector to move to.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void MoveLinearAsync(Vector vectorA, Vector vectorB, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => MoveLinear(vectorA, vectorB));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Moves the conveyor belt.
        /// </summary>
        /// <param name="velocity">Speed/direction of the conveyor belt.</param>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void MoveConveyorBeltAsync(int velocity, Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => MoveConveyorBelt(velocity));

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Opens the gripper.
        /// </summary>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void OpenGripperAsync(Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => OpenGripper());

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        /// <summary>
        /// Closes the gripper.
        /// </summary>
        /// <param name="callback">
        /// Callback which returns <c>true</c> if the method has been succesfully executed; otherwise, <c>false</c>.
        /// </param>
        public void CloseGripperAsync(Action<bool> callback = null)
        {
            var task = Task.Factory.StartNew(() => CloseGripper());

            if (callback != null)
            {
                task.ContinueWith(t => callback(t.Result));
            }
        }

        #endregion
    }
}
